using System;
using System.Collections.Generic;
using System.Text;
using surfInterop;
using System.Drawing;
using Blimpbots.Recognition;
using DotNetMatrix;

namespace Blimpbots.Recognition.Algorithms
{
    //  Generative tracking:
    //   http://johnwinn.org/Publications/papers/GenTracking2004.pdf

    public class Generative
    {
        private class BackgroundKeypoint
        {

            private int _latestIdx; // Index of this keypoint in the latest frame
            public int Idx { get { return _latestIdx; } }
            public double X, Y;
            public int Strength;
            public void AddObservation(double X, double Y, int Idx)
            {
                // Migrate the point with the observation
                this.X = X;
                this.Y = Y;
                _latestIdx = Idx;
            }
            public void Strengthen()
            {
                Strength++;
                if (Strength >= 5) Strength = 5;
            }
            public void Weaken()
            {
                Strength--;
                if (Strength < 0) Strength = 0;
            }
            public BackgroundKeypoint(double X, double Y, int Idx)
            {
                // Grab the values now to reduce a layer of indirection later
                this.X = X;
                this.Y = Y;
                this._latestIdx = Idx;
                
                // Initialized to 0
                this.Strength = 0;
            }
            public double Dist(double X, double Y)
            {
                return Math.Sqrt(Math.Pow(this.X - X, 2) + Math.Pow(this.Y - Y, 2));
            }
        }

        List<int> _latestBkgIndices = new List<int>();

        // All background points that we're currently monitoring
        private List<BackgroundKeypoint> cumulativeBkg = new List<BackgroundKeypoint>();

        public int BackgroundCount { get { return cumulativeBkg.Count; } }

        private List<SurfKeypoint> _recentKeypoints = new List<SurfKeypoint>();

        // Holds background keypoints from the cumulative list which were *just* observed
        private List<BackgroundKeypoint> _observedBkg = new List<BackgroundKeypoint>();
        private List<BackgroundKeypoint> _frameBkg = new List<BackgroundKeypoint>();

        private int _lastKeypointCount = 0;

        private int _backgroundStrength = 0;
        private float _objectPadding = 0;

        /// <summary>
        /// Creates a new Generative fg/background tracking model.
        /// </summary>
        /// <param name="BackgroundStrength">Number of repeated observations before a point is considered in the background.</param>
        public Generative(int BackgroundStrength, float ObjectPadding)
        {
            _objectPadding = ObjectPadding;
            _backgroundStrength = BackgroundStrength;
        }

        // Need to figure out what represents the background vs. 
        //   what represents the target object.

        public List<int> objKpts = new List<int>();

        public void ObserveObjects(List<LikelyObject> detectedObjects)
        {
            List<BackgroundKeypoint> inferredBkg, inferredObj = new List<BackgroundKeypoint>();
            inferredBkg = new List<BackgroundKeypoint>(_frameBkg);

            foreach (LikelyObject obj in detectedObjects)
            {
                List<BackgroundKeypoint> curObj;
                ClassifyKeypoints(obj, inferredBkg, out curObj, out inferredBkg);
                inferredObj.AddRange(curObj);
            }

            foreach (BackgroundKeypoint k in inferredBkg) k.Strengthen();
            foreach (BackgroundKeypoint k in inferredObj) k.Weaken();

            _frameBkg.Clear();

            RemoveWeakPoints();
        }

        public void DrawBackground(Graphics g)
        {
            foreach (BackgroundKeypoint kp in cumulativeBkg)
            {
                if(kp.Strength > 2)
                    g.FillEllipse(Brushes.Chartreuse, (float)kp.X - 2f, (float)kp.Y - 2f, 4f, 4f);
            }
        }

        public void ObserveRegion(SurfKeypoint[] dataKeypoints, Rectangle r)
        {
            // Shrink the region because surf keypoints
            // tend not to get detected at the edge of regions
            // even if the underlying pattern is actually there

            r.Inflate(-10, -10);

            _lastKeypointCount = dataKeypoints.Length;
            _observedBkg.Clear();

            LinkedList<BackgroundKeypoint> lookFor = new LinkedList<BackgroundKeypoint>();

            foreach (BackgroundKeypoint k in cumulativeBkg)
            {
                // Check if the point is inside the rectangle
                if (((double)r.X <= k.X) && ((double)r.Y < k.Y)
                 && ((double)r.Right >= k.X) && ((double)r.Bottom >= k.Y))
                {
                    lookFor.AddLast(k);
                }
            }

            List<BackgroundKeypoint> newBackground = new List<BackgroundKeypoint>();

            for(int idx = 0; idx < dataKeypoints.Length; idx++)
            {
                SurfKeypoint k = dataKeypoints[idx];

                bool found = false;
                LinkedListNode<BackgroundKeypoint> node = lookFor.First;
                while (node != null) /* When empty, this check will skip the loop */
                {
                    if (node.Value.Dist(k.x, k.y) < 2.0) // TODO: make parameter
                    {
                        found = true;
                        node.Value.AddObservation(k.x, k.y, idx);
                        _observedBkg.Add(node.Value);
                        LinkedListNode<BackgroundKeypoint> toDelete = node;
                        node = node.Next;
                        lookFor.Remove(toDelete);
                    }
                    else
                        node = node.Next;

                    if (found) break; // quit once we found it
                }

                if (!found)
                {
                    BackgroundKeypoint newKpt = new BackgroundKeypoint(k.x, k.y, idx);
                    newBackground.Add(newKpt);
                    _observedBkg.Add(newKpt);
                }

            } // end loop over all datakeypoints


            // keep a full-frame list of keypoints
            _frameBkg.AddRange(_observedBkg);

            // Add the batch of newly-found bkg points
            cumulativeBkg.AddRange(newBackground);

            // Weaken the not-found background points
            foreach (BackgroundKeypoint node in lookFor)
                node.Weaken();

        } // end function

        /// <summary>
        /// Returns indices of data keypoints likely corresponding to the background.
        /// Indices correspond to those of the latest call to ObserveFrame.
        /// </summary>
        public int[] BackgroundPrediction
        {
            get
            {
                List<int> result = new List<int>();
                foreach (BackgroundKeypoint k in _observedBkg)
                    if (k.Strength >= _backgroundStrength) result.Add(k.Idx);
                return result.ToArray();
            }
        }

        public int[] ForegroundPrediction
        {
            get 
            {
                bool[] bgMask = new bool[_lastKeypointCount];
                int[] bgPrediction = BackgroundPrediction;
                foreach (int idx in bgPrediction)
                {
                    bgMask[idx] = true;
                }
                int i = 0;
                int[] fgIndices = new int[_lastKeypointCount - bgPrediction.Length];
                for(int j = 0; j < _lastKeypointCount; j++)
                    if (bgMask[j] == false)
                    {
                        fgIndices[i] = j;
                        i++;
                    }
                return fgIndices;
            }
        }

            private void RemoveWeakPoints()
        {
            // In reverse order (for speed), delete very weak background pts
            for (int i = cumulativeBkg.Count - 1; i >= 0; i--)
                if (cumulativeBkg[i].Strength <= 0) cumulativeBkg.RemoveAt(i);
        }

        //public GenerativeExample GenerateExample(LikelyObject detectedObject, SurfKeypoint[] keypoints)
        //{
        //    List<SurfKeypoint> objectKeypoints = new List<SurfKeypoint>();
        //    List<bool> realKeypoint = new List<bool>();

        //    RectangleF modelRegion = detectedObject.Example.BoundingBox;
        //    modelRegion.Offset(-modelRegion.Left, -modelRegion.Top);
        //    modelRegion.Inflate(_objectPadding, _objectPadding);
        //    GeneralMatrix inverseAffine = detectedObject.AffineModel.H.Inverse();

        //    List<List<CorrespondingKeypoint>> corresp = new List<List<CorrespondingKeypoint>>();

        //    for(int i = 0; i < keypoints.Length; i++)
        //    {
        //        SurfKeypoint k = keypoints[i];
        //        GeneralMatrix kMat = new GeneralMatrix(new double[] { k.x, k.y, 1.0 }, 3);
        //        GeneralMatrix mdlCoord = inverseAffine.Multiply(kMat);
        //        mdlCoord = mdlCoord.HomogeneousNormalizeEquals();
        //        PointF mdlPt = new PointF((float)(mdlCoord.Array[0][0]),
        //                                  (float)(mdlCoord.Array[1][0]));

        //        if (modelRegion.Contains(mdlPt))
        //        {
        //            objectKeypoints.Add(k);

        //            bool found = false;
        //            foreach(KeypointMatch kpm in detectedObject.AffineModel.Inliers)
        //            {
        //                if (kpm.dataIdx != i) continue;
        //                found = true;
        //                corresp.Add(
        //                    detectedObject.Example.Correspondences[kpm.modelIdx]);
        //            }

        //            realKeypoint.Add(found); // true=keypoint was in the last model

        //            if (!found)
        //                corresp.Add(new List<CorrespondingKeypoint>(0));
        //        }
        //    }

        //    // How Correspondences are Inferred for Generative Examples:
        //    //
        //    //  First, create list of correspondence lists
        //    //     (since we don't know how many keypoints will end up in
        //    //      the new model)
        //    //
        //    //  Next, for each keypoint in the new model, check if
        //    //     it was an inlier in the source object.
        //    //     
        //    //      If so, use the source object's model correspondences
        //    //      as the generative model's correspondences.
        //    //      If not, assume there are no correspondences.
        //    //
        //    //  Finally, convert the list of lists into an array of lists
        //    //      and construct the generative example with it.

        //    GenerativeExample eg = new GenerativeExample(detectedObject.Example, corresp.ToArray(), detectedObject.AffineModel.Transform);

        //    # region Store the shape
        //    PointF[] newShape = detectedObject.Example.ProjectShape(detectedObject.AffineModel.Transform);
        //    eg.ObjectOutline = new List<Point>(newShape.Length);
        //    foreach (PointF pt in newShape)
        //    {
        //        // TODO: convert these to PointF in the model
        //        // so we don't incur tons of error here
        //        eg.ObjectOutline.Add(new Point((int)pt.X, (int)pt.Y));
        //    }
        //    #endregion

        //    System.Drawing.RectangleF bound = detectedObject.Example.BoundingBox;

        //    foreach (KeyValuePair<string, NamedPoint> kvp in detectedObject.Example.NamedPoints)
        //    {
        //        double x = kvp.Value.Point.X - bound.Left;
        //        double y = kvp.Value.Point.Y - bound.Top;
                
        //        // Note: this would seem correct, but the scale is too noisy to rely
        //        //       on this.
        //        //double d = kvp.Value.Distance / detectedObject.AffineModel.Scale;

        //        double d = kvp.Value.Distance;

        //        Point2D pt2Dd = Geometry.ProjectPoint(x, y, detectedObject.AffineModel.H);
        //        eg.NamedPoints.Add(kvp.Key, new NamedPoint(new Point((int)pt2Dd.x, (int)pt2Dd.y), d));
        //    }

        //    eg.ModelKeypoints = objectKeypoints;
        //    eg.KeypointValidity = realKeypoint.ToArray();

        //    eg.Bound();

        //    return eg;

        //}


        /// <summary>
        /// Given some keypoints and some objects with rectangular shapes mapped onto the 
        /// framespace with an affine transform, this function classifies each keypoint as either
        /// part of the background, or part of some object.
        /// </summary>
        /// <param name="detectedObjects">Objects believed in the frame.</param>
        /// <param name="keypoints">Keypoints to be classified.</param>
        /// <param name="objectKeypoints">Resulting keypoints in the object.</param>
        /// <param name="backgroundKeypoints">Resulting keypoints in the background.</param>
        private void ClassifyKeypoints(LikelyObject detectedObject,
            List<BackgroundKeypoint> keypoints, 
            out List<BackgroundKeypoint> objectKeypoints, 
            out List<BackgroundKeypoint> backgroundKeypoints)
        {
            objectKeypoints = new List<BackgroundKeypoint>(keypoints.Count);
            backgroundKeypoints = new List<BackgroundKeypoint>(keypoints.Count);

            // Find the mapping from data space back to model space
            // OLD CODE FOR AFFINE TRANSFORM
            //RectangleF modelRegion = detectedObject.Example.BoundingBox;
            //modelRegion.Offset(-modelRegion.Left, -modelRegion.Top);
            //modelRegion.Inflate(_objectPadding, _objectPadding);
            //GeneralMatrix inverseAffine = detectedObject.AffineModel.H.Inverse();
            
            RectangleF objectBox = detectedObject.RubberBand();

            foreach(BackgroundKeypoint k in keypoints)
            {
                if(objectBox.Contains((float)k.X, (float)k.Y))
                    objectKeypoints.Add(k);
                else
                    backgroundKeypoints.Add(k);

                //GeneralMatrix kMat = new GeneralMatrix(new double[] { k.X, k.Y, 1.0 }, 3);

                //GeneralMatrix mdlCoord = inverseAffine.Multiply(kMat);
                //double norm = mdlCoord.Array[2][0];
                //PointF mdlPt = new PointF((float)(mdlCoord.Array[0][0] / norm),
                //                          (float)(mdlCoord.Array[1][0] / norm));
                //if (modelRegion.Contains(mdlPt))
                //    objectKeypoints.Add(k);
                //else
                //    backgroundKeypoints.Add(k);
            }

        } // end function

        
    }
}
